package gnss

import (
	"dgo/goutils/utils"
	"math"
)

type IgsSSR_IGM06_Sig struct {
	SigId         byte
	SigIndicator1 byte
	SigIndicator2 byte
	SigDisCounter byte

	// 米
	PhaseBias float64
}

type IgsSSR_IGM06 struct {
	SatId byte

	// 角度
	YawAngle float64

	//角度
	YawRate float64

	Sigs [32]IgsSSR_IGM06_Sig
}

type IgsSSR_IGM06_List [SATID_MAX]IgsSSR_IGM06

func (this *IgsSSR_IGM06_List) Initial() {
	for i := 0; i < len(this); i++ {
		for j := 0; j < len(this[i].Sigs); j++ {
			this[i].Sigs[j].PhaseBias = 999
		}
	}
}

func (this IgsSSR_IGM06_Sig) Valid() bool {
	if this.SigId != 255 && this.SigId != 0 {
		return true
	}

	if this.PhaseBias != 999 {
		return true
	}
	return false
}

func (this IgsSSR_IGM06) sigCount() int {
	r := 0
	for i := 0; i < len(this.Sigs); i++ {
		rec := this.Sigs[i]
		if rec.Valid() {
			r++
		}
	}
	return r
}

func (this *IgsSSR_IGM06_List) Count() int {
	r := 0
	for i := 0; i < len(this); i++ {
		rec := this[i]
		if rec.SatId != 255 && rec.SatId != 0 {
			r++
		}
	}
	return r
}

func (this *IgsSSR_IGM06_List) CheckGetSat(satid byte) *IgsSSR_IGM06 {
	sat := &this[satid-1]
	if sat.SatId != satid {
		sat.SatId = satid
	}
	return sat
}

func (this *IgsSSR_IGM06_List) SetValue(satid byte, sigid byte, v float64) {
	if satid == 0 {
		return
	}
	sat := &this[satid-1]
	sat.SatId = satid
	sigv := &sat.Sigs[sigid]
	sigv.SigId = sigid
	sigv.PhaseBias = v
}

func EncodeIgsSSR7(head *IgsSSRHead, lst *IgsSSR_IGM06_List, bb *utils.BytesBuilder, startByteN int) int {
	satn := uint32(lst.Count())
	var startBitN int = 0
	head.MsgID = 4076
	if head.NaviSys == SYS_GPS {
		startBitN = head.EncodeSSR7Head(bb, startByteN, 26, satn)
	} else if head.NaviSys == SYS_GLO {
		startBitN = head.EncodeSSR7Head(bb, startByteN, 46, satn)
	} else if head.NaviSys == SYS_BD {
		startBitN = head.EncodeSSR7Head(bb, startByteN, 106, satn)
	} else if head.NaviSys == SYS_GAL {
		startBitN = head.EncodeSSR7Head(bb, startByteN, 66, satn)
	} else if head.NaviSys == SYS_QZS {
		startBitN = head.EncodeSSR7Head(bb, startByteN, 86, satn)
	}
	startBitN = EncodeSSR7Sat(head, lst, bb, startByteN, startBitN)
	byteN := int(math.Ceil(float64(startBitN) / 8))
	bb.Delete2End(startByteN + byteN)
	return byteN
}

func EncodeRtcmIgsSSR7(head *IgsSSRHead, lst *IgsSSR_IGM06_List, bb *utils.BytesBuilder) {
	idx0 := bb.Len()
	bb.Write([]byte{0xD3, 0, 0})
	ByteN := EncodeIgsSSR7(head, lst, bb, bb.Len())
	bb.SetBitU16(idx0, 8+6, 10, uint16(ByteN))

	bb.CheckSize(bb.Len(), 3)
	crcBuf := bb.Buff(idx0, ByteN+3) // 3:同步标记 + 保留位 + 长度（RTCM消息体）(10bit)
	crc1 := utils.CRC24QBuf(crcBuf)
	bb.SetBitU32(idx0+3+ByteN, 0, 24, uint32(crc1))
	bb.Delete2End(idx0 + 3 + ByteN + 3)
}

/*
GNSS SSR Phase Bias Message IGM06
*/
func EncodeSSR7Sat(head *IgsSSRHead, lst *IgsSSR_IGM06_List, bb *utils.BytesBuilder, startByteN int, startBitN int) int {
	for i := 0; i < len(lst); i++ {
		sat := lst[i]
		if sat.SatId > 0 {
			n := sat.sigCount()
			if n == 0 {
				continue
			}

			bb.CheckSize(startByteN, (startBitN+28+32*n)/8+1)
			// GNSS Satellite ID
			bb.SetBitU8(startByteN, startBitN, 6, byte(sat.SatId))
			startBitN += 6

			/* No. of Biases Processed */
			bb.SetBitU8(startByteN, startBitN, 5, byte(n))
			startBitN += 5

			//utils.Log.DebugTypef("", "phase %s%.2d, n:%d", GetNavSysCode(head.NaviSys), sat.SatId, n)

			/* Yaw Angle 参考rtklib解码, 不能有负数 */
			if sat.YawAngle < 0 {
				sat.YawAngle += 360
			}
			bb.SetBitU16(startByteN, startBitN, 9, uint16(math.Round(sat.YawAngle/180*256)))
			startBitN += 9

			/* Yaw Rate */
			bb.SetBitU16(startByteN, startBitN, 8, uint16(math.Round(sat.YawRate/180*8192)))
			startBitN += 8

			for j := 0; j < len(sat.Sigs); j++ {
				sig := sat.Sigs[j]
				if sig.Valid() {
					/**	GNSS Signal and Tracking Mode Identifier */
					bb.SetBitU8(startByteN, startBitN, 5, byte(sig.SigId))
					startBitN += 5

					/* Signal Integer Indicator */
					bb.SetBitU8(startByteN, startBitN, 1, byte(sig.SigIndicator1))
					startBitN += 1

					/* Signals Wide-Lane Integer  Indicator */
					bb.SetBitU8(startByteN, startBitN, 2, byte(sig.SigIndicator2))
					startBitN += 2

					/** Signal Discontinuity Counter */
					bb.SetBitU8(startByteN, startBitN, 4, byte(sig.SigDisCounter))
					startBitN += 4

					/**	Phase Bias */
					v := int32(math.Round(sig.PhaseBias * 10000))
					//if head.NaviSys == SYS_BD && (sat.SatId == 6 || sat.SatId == 9) {
					//	utils.Log.DebugTypef("SSR", "Phase Bias %d %d %d", sat.SatId, sig.SigId, v)
					//}
					bb.SetBitI32(startByteN, startBitN, 20, v)
					startBitN += 20
				}
			}
		}
	}

	return startBitN
}
